/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Implementation of ObjectDB interface Server.
 * \author Antti Maula <antti.maula@tkk.fi>
 *
 * $Id: ObjectDBServer.cpp,v 1.2 2009-09-01 12:56:27 amaula Exp $
 *
 */
#include "ObjectDBServer.hpp"
#include "ObjectDBData.hpp"

//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
//*****************************************************************************
using namespace MaCI::ObjectDB;
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

CObjectDBServer::CObjectDBServer(MaCICtrl::CMaCICtrlServer *aMaCICtrlServerPtr, 
                           const int aInterfaceMinor,
                           const std::string &aDescription,
                           const std::string &aInstanceName)
  : CMaCI(aMaCICtrlServerPtr, GIMI_PROTOCOL_MACI_OBJECTDB, aInterfaceMinor, aInstanceName),
    CSync(1,1),
    CThread(1),
    iUnprocessedCommandsArray(),
    iServiceDescription(aDescription),
    iObjectDBSequenceNumber(0),
    iObjectDBServerCallback(NULL)
{
}
//*****************************************************************************

CObjectDBServer::~CObjectDBServer()
{
  DoClose();
}
//*****************************************************************************

MaCI::EMaCIError CObjectDBServer::DoOpen(void)
{
  EMaCIError result = KMaCIError;
  assert(this != NULL);
  assert(iGIMIPtr != NULL);
  
  // Execute only if server is not active.
  if (!IsActive()) {
    // Register service.
    RegisterProvidedService("MaCI-ObjectDBServer; '" + iServiceDescription + "'");
    RegisterAcceptedService("MaCI-ObjectDBServer-Cmd; '" + iServiceDescription + "'");
    
    // Start the handler Thread
    SetActive( true );
    CThread::RunThread();
    
    result = KMaCIOK;
    
    dPrint(4,"Succesfully Open():ed ObjectDBServer instance %p.", this);
  }
    
  return result;
}
//*****************************************************************************

MaCI::EMaCIError CObjectDBServer::DoClose(void)
{
  EMaCIError result = KMaCIError;
  assert(this != NULL);
  assert(iGIMIPtr != NULL);

  // Execute block only if server is currently active.
  if (IsActive()) {
    // Stop!
    SetActive(false);

    // Stop the handler Thread
        /* CThread::CancelThread(); //Removed by script */
        CThread::WaitThread();    

    // Unregister services
    UnregisterProvidedService();
    UnregisterAcceptedService();
    
    dPrint(4,"Succesfully Close():ed ObjectDBServer instance %p", this);
  }
  
  return result;
}
//*****************************************************************************

MaCI::EMaCIError CObjectDBServer::SendReply(const CObjectDBData &aData, 
                                            const SMessageSource &aSource)
{
  EMaCIError result = KMaCIError;
  // Check that server is active.
  if (IsActive()) {
    
    // Check that compulsory element is found. (This is ofcourse interface specific)
    if (aData.GetCommand()) {
      
      // Send
      result = SendSynchronousMessageReply(aData.GetBinBagContainerPtr()->GetBuffer(),
                                           aData.GetBinBagContainerPtr()->GetBufferSize(),
                                           aSource);
      
    } else {
      dPrint(2,"No 'ObjectDBEvent' element in provided CObjectDBData object. Will not send!");
      result = KMaCIInvalidFormat;
      
    }
    
  } else {
    dPrint(2,"Unable to send ObjectDBData - Interface is not active! (Open() not called?)");
    result = KMaCINotActive;
    
  }
  return result;
}
//*****************************************************************************

MaCI::EMaCIError CObjectDBServer::WaitCommand(CObjectDBData &aData, 
                                              SMessageSource &aSource,
                                              const unsigned int aTimeout_ms)
{
  EMaCIError result = KMaCIError;
  int time_left = aTimeout_ms;
  
  // Fallback in case the class is not active.
  if (!IsActive()) return MaCI::KMaCINotActive;
  
  Lock();
  
  // First; check is the array empty; if it is, we must do waiting.
  if (iUnprocessedCommandsArray.empty()) {
    unsigned int time_begin = ownTime_get_ms();
    
    // Wait for the new data to arrive
    do {
      Wait(time_left);
      time_left = aTimeout_ms - ownTime_get_ms_since(time_begin);
    } while(IsActive() && 
            iUnprocessedCommandsArray.empty() && 
            time_left > 0);
  }

  // Wait may have or may not have executed before; but at this point
  // we are not interested, only test whether there now is commands in
  // buffer or not.
  if (iUnprocessedCommandsArray.size()) {
    // Messages in buffer, take the oldest and process it.
    const typeof(*iUnprocessedCommandsArray.begin()) &p = iUnprocessedCommandsArray.front();

    // Copy oldest data and messagesource to caller.
    aData.Copy(p.first);
    aSource = p.second;

    // Erase the oldest element
    iUnprocessedCommandsArray.pop_front();

    // Mark result OK.
    result = KMaCIOK;

  } else if (time_left <= 0) {
    result = KMaCITimeout;

  }


    

  Unlock();

  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

int CObjectDBServer::ProcessMessage(const CObjectDBData &aMessage, const SMessageSource &aSource)
{
  int result = 0;
  gim::binbag::CBinBag bb;
  CObjectDBData id;
  
  // If this function gets called, the data element is valid ObjectDBData
  // and the calling client has the permission to access this
  // interface.
  CObjectDBData ddr;
  ddr.CreateInternalBinBag();
  
  const TCommand *cmd = aMessage.GetCommand();
  if (cmd) {
    dPrint(ODTEST,"Got command: '%d'", cmd->cmd);
    switch(cmd->cmd) {
    case KCommandGenerateRealObject:
    case KCommandGetRealObjectInformation:
    case KCommandSetRealObjectInformation:
    case KCommandGetRealObjectPose:
    case KCommandSetRealObjectPose:
    case KCommandGetRealObjectBoundingBox:
    case KCommandSetRealObjectBoundingBox:
    case KCommandGetRealObjectsBySelection: {
      // Do Processing; Store last 
      Lock();
      typeof(*iUnprocessedCommandsArray.begin()) p;
      p.first.Copy(aMessage);
      p.second = aSource;
      iUnprocessedCommandsArray.push_back(p);
      Notify();
      Unlock();
      
      // Call the handler if set.
      if (iObjectDBServerCallback) {
        dPrint(8,"Got Callback for ObjectDBDataEvent - executing!");
        iObjectDBServerCallback->OnObjectDBDataEvent(aMessage,
                                                     *this);
        dPrint(8,"Callback for ObjectDBDataEvent - execution complete!");
      } 
      break;
    }
      
    default:
      dPrint(2,"Unsupported ObjectDBServer command %d received. Ignoring.",
             cmd->cmd);
      break;
    }
  } else {
    dPrint(ODTEST,"NULL Command to ProcessMessage function");
  }
  
  return result;
}
//*****************************************************************************

int CObjectDBServer::ThreadFunction(const int aThreadNumber)
{
  int result = 0;
  const unsigned int hard_error_count_limit = 10;
  unsigned int hard_error_count = 0;
  EMaCIError e;
  CObjectDBData dd;
  SMessageSource ms;

  assert(aThreadNumber == 0);
  assert(iGIMIPtr != NULL);
  
  dPrint(8, "GIMIMessage handler started");
  
  // Run as long as the thread should be active
  while(IsActive()) {
    dd.Reset();
    
    // Call the ReceiveMessage wrapper with 1s timeout. This template
    // function model is able to convert to correct type
    // automatically.
    
    e = ReceiveInterfaceMessage(dd, ms, 2000);
    if (e == KMaCIOK) {
    
      ProcessMessage(dd, ms);
      
      // Reset error counter.
      if (hard_error_count) hard_error_count = 0;
      
    } else if (e != KMaCITimeout) {
      // Increment hard error count
      ++hard_error_count;
      
      // Check whether the hard error count limit is still under limit. 
      if (hard_error_count < hard_error_count_limit) {
        // Other error, try to recover by sleeping one second.
        dPrint(10,"ProcessMessage returned error %u, trying to recover by sleeping 1000ms (%u attempts left)",
               e, hard_error_count_limit-hard_error_count);
        ownSleep_ms(1000);

      } else {
        dPrint(3,"Error Recovery failed, ObjectDBServer ThreadFunction terminating...");

        // Break the while().
        break;
      }
      
    }
  }

  dPrint(8, "GIMIMessage handler stopped");
  SetActive(false);

  return result;
}

//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

